/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-03-01     86188       the first version
 */
#include "head_rtthread.h"
#include "head_user.h"
#include "math.h"

/* 机体系向导航系向量转换 */
static void Body_Frame_To_Navi_Frame(Axis3f *bf, Axis3f *nf, float (*rMat)[3])
{
    nf->x = rMat[0][0]*bf->x + rMat[0][1]*bf->y + rMat[0][2]*bf->z;
    nf->y = rMat[1][0]*bf->x + rMat[1][1]*bf->y + rMat[1][2]*bf->z;
    nf->z = rMat[2][0]*bf->x + rMat[2][1]*bf->y + rMat[2][2]*bf->z;
}
/* 导航系向机体系向量转换 */
static void Navi_Frame_To_Body_Frame(Axis3f *bf, Axis3f *nf, float (*rMat)[3])
{
    bf->x = rMat[0][0]*nf->x + rMat[1][0]*nf->y + rMat[2][0]*nf->z;
    bf->y = rMat[0][1]*nf->x + rMat[1][1]*nf->y + rMat[2][1]*nf->z;
    bf->z = rMat[0][2]*nf->x + rMat[1][2]*nf->y + rMat[2][2]*nf->z;
}

/* 导航系向机体系xy向量转换 yaw:rad*/
static void Navi_To_Body_2(Axis2f *bf, Axis2f *nf, const float yaw)
{
    bf->x = nf->x*cosf(yaw/57.3f) + nf->y*sinf(yaw/57.3f);
    bf->y = nf->y*cosf(yaw/57.3f) - nf->x*sinf(yaw/57.3f);
}

static Axis3f tmp, tmpbf, tmpnf;
static void RemoveGravity(Axis3f *bf, Axis3f *nf, float (*rMat)[3])
{
    Body_Frame_To_Navi_Frame(&tmp, nf, rMat);

    /* 去除重力加速度 */
    nf->z -= 980.0f;

    Navi_Frame_To_Body_Frame(bf, nf, rMat);
}

static EKF_HandleTypedef EKF_Handle = {0};
SINS_HandleTypedef BODY;

//速度预估和位置预估
void Position_Estimation(float dt)
{
    static uint8_t isInitialized = 0;
    static float Q[4]={0.00001,0.0001,0.0001,0.064};    //过程噪声方差//a^2*{dT^4/4,dT^3/2,dT^3/2,dT^2}
    static float Pre_cov[4]={256, 32, 32, 16}; //上一次协方差

    //初始化预估器
    if(!isInitialized)
    {
        isInitialized = 1;
        EKF_init(&EKF_Handle, 10, Pre_cov, Q);                       //高度卡尔曼参数初始化
    }

    //更新预估器的加速度
    /* 获取加速度计原始惯性数据 */
    tmp.x = -IMU_Handle.imu_data->acce.x;
    tmp.y = -IMU_Handle.imu_data->acce.y;
    tmp.z = -IMU_Handle.imu_data->acce.z;
    /* 转换到导航系移除重力,得到机体系及导航系的惯性数据 */
    RemoveGravity(&tmpbf, &tmpnf, INS_Handle.rMat);
    BODY.nfAcce.x = -tmpnf.y;
    BODY.nfAcce.y = -tmpnf.x;
    BODY.nfAcce.z =  tmpnf.z;
    BODY.bfAcce.x = -tmpbf.y;
    BODY.bfAcce.y = -tmpbf.x;
    BODY.bfAcce.z =  tmpbf.z;
    /* 数据融合 */
    /* 高度融合 */
    EKF_Handle.R[0] = 100;
    if(EKF_Handle.R[0] == 0)
    {
        EKF_Handle.R[0] = 10000;
    }
    EKF_Update(&EKF_Handle, *Sensor_Handle.height, BODY.nfAcce.z, dt, 'Z');
    /* 水平左右融合 */
    EKF_Handle.R[0] = BODY.PosR.x;
    if(EKF_Handle.R[0] == 0)
    {
        EKF_Handle.R[0] = 10000;
    }
    EKF_Update(&EKF_Handle, BODY.PosObser.x, BODY.nfAcce.x, dt, 'X');
    /* 水平前后融合 */
    EKF_Handle.R[0] = BODY.PosR.y;
    if(EKF_Handle.R[0] == 0)
    {
        EKF_Handle.R[0] = 10000;
    }
    EKF_Update(&EKF_Handle, BODY.PosObser.y, BODY.nfAcce.y, dt, 'Y');

    /* 获取最优估计数据 */
    for(uint8_t i=0; i<3; i++)
    {
        BODY.nfPos.axis[i]    = EKF_Handle.Position.axis[i];
        BODY.nfSpeed.axis[i]  = EKF_Handle.Speed.axis[i];
        BODY.Acce.axis[i]   = EKF_Handle.Acce.axis[i];
    }
    Navi_Frame_To_Body_Frame(&BODY.bfSpeed, &BODY.nfSpeed, INS_Handle.rMat);    //导航系速度转为机体系
}

static int SINS_init(void)
{
    BODY.Navi_To_Body_2 = Navi_To_Body_2;
    BODY.gyro           = &IMU_Handle.imu_data->gyro;
    BODY.angle          = INS_Handle.imuAngle;

    return 0;
}
INIT_APP_EXPORT(SINS_init);
